package org.noote.libs.spidy.anim;

import org.noote.libs.robot.wild.WildRobot_Animation;
import org.noote.libs.robot.wild.WildRobot_AnimationContext;

public class Spidy_WildAnim_Stand extends WildRobot_Animation {
	
	protected boolean bDoPlacement = false;
	protected float fServoOldPos[] = new float [6];
	protected float fStepTime = 0.0f;
	protected int iStep = 0;
	
	protected float fCoxyLow = 0, fCoxyHigh = 0;
	protected float fFemurLow = 20, fFemurHigh = 65;
	protected float fTibiaLow = 40, fTibiaHigh = 75;

	public Spidy_WildAnim_Stand()
	{
		super("Stand");
	}
	
	@Override
	public boolean create() {

		// ?
		
		return true;
	}
	
	// TODO add a spline ?
	float lerp(float min, float max, float lerp)
	{
		lerp = lerp>1?1:lerp<0?0:lerp; // clamp [0,1]
		return min + lerp*(max-min);
	}

	@Override
	public boolean startAnimation(WildRobot_AnimationContext context)
	{
		// reset anim step
		fStepTime = 0.0f;
		iStep = 0;

		bDoPlacement = checkPlacement(context);
		
		return !doAnimation(0, context);
	}
	
	@Override
	public boolean doAnimation(float fSlice, WildRobot_AnimationContext context)
	{
		if(bDoPlacement)
		{
			if(animPlacement(fSlice, context))
				bDoPlacement = false;
			return false;
		}
		return animStand(fSlice, context);
	}
	
	public boolean animStand(float fSlice, WildRobot_AnimationContext context)
	{
		fStepTime+=fSlice;
		
		context.setPosition(Spidy_BaseSkeleton.LEFT_FRONT_COXA,   -fCoxyLow);
		context.setPosition(Spidy_BaseSkeleton.LEFT_FRONT_FEMUR,  -fFemurLow);
		context.setPosition(Spidy_BaseSkeleton.LEFT_FRONT_TIBIA,  -fTibiaLow);

		context.setPosition(Spidy_BaseSkeleton.RIGHT_FRONT_COXA,   fCoxyLow);
		context.setPosition(Spidy_BaseSkeleton.RIGHT_FRONT_FEMUR,  fFemurLow);
		context.setPosition(Spidy_BaseSkeleton.RIGHT_FRONT_TIBIA,  fTibiaLow);
		
		context.setPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_COXA,   -fCoxyLow);
		context.setPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_FEMUR,  -fFemurLow);
		context.setPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_TIBIA,  -fTibiaLow);

		context.setPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_COXA,   fCoxyLow);
		context.setPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_FEMUR,  fFemurLow);
		context.setPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_TIBIA,  fTibiaLow);
		
		context.setPosition(Spidy_BaseSkeleton.LEFT_BACK_COXA,   -fCoxyLow);
		context.setPosition(Spidy_BaseSkeleton.LEFT_BACK_FEMUR,  -fFemurLow);
		context.setPosition(Spidy_BaseSkeleton.LEFT_BACK_TIBIA,  -fTibiaLow);

		context.setPosition(Spidy_BaseSkeleton.RIGHT_BACK_COXA,   fCoxyLow);
		context.setPosition(Spidy_BaseSkeleton.RIGHT_BACK_FEMUR,  fFemurLow);
		context.setPosition(Spidy_BaseSkeleton.RIGHT_BACK_TIBIA,  fTibiaLow);

		if(fStepTime>0.25f)
		{
			// end animation after 0.25s
			return true;
		}
		
		return false;
	}

	protected boolean checkPlacement(WildRobot_AnimationContext context)
	{
		float fCoxyT1 = fCoxyHigh, fFemurT1 = fFemurLow, fTibiaT1 = fTibiaLow;
		float fCoxyT2 = fCoxyLow, fFemurT2 = fFemurLow, fTibiaT2 = fTibiaLow;

		if(context.getPosition(Spidy_BaseSkeleton.LEFT_FRONT_COXA) != -fCoxyT1) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.LEFT_FRONT_FEMUR) != -fFemurT1) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.LEFT_FRONT_TIBIA) != -fTibiaT1) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_COXA) != -fCoxyT2) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_FEMUR) != -fFemurT2) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_TIBIA) != -fTibiaT2) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.LEFT_BACK_COXA) != -fCoxyT1) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.LEFT_BACK_FEMUR) != -fFemurT1) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.LEFT_BACK_TIBIA) != -fTibiaT1) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.RIGHT_FRONT_COXA) != fCoxyT2) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.RIGHT_FRONT_FEMUR) != fFemurT2) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.RIGHT_FRONT_TIBIA) != fTibiaT2) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_COXA) != fCoxyT1) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_FEMUR) != fFemurT1) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_TIBIA) != fTibiaT1) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.RIGHT_BACK_COXA) != fCoxyT2) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.RIGHT_BACK_FEMUR) != fFemurT2) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.RIGHT_BACK_TIBIA) != fTibiaT2) return true;
		
		return false;
	}

	public boolean animPlacement(float fSlice, WildRobot_AnimationContext context)
	{
		float fStepPlacementDuration = 0.125f;
		
		if(fStepTime == 0)
		{
			switch(iStep)
			{
			case 0:
				fServoOldPos[0] = context.getPosition(Spidy_BaseSkeleton.LEFT_FRONT_COXA);
				fServoOldPos[1] = context.getPosition(Spidy_BaseSkeleton.LEFT_FRONT_FEMUR);
				fServoOldPos[2] = context.getPosition(Spidy_BaseSkeleton.LEFT_FRONT_TIBIA);
				fServoOldPos[3] = context.getPosition(Spidy_BaseSkeleton.RIGHT_BACK_COXA);
				fServoOldPos[4] = context.getPosition(Spidy_BaseSkeleton.RIGHT_BACK_FEMUR);
				fServoOldPos[5] = context.getPosition(Spidy_BaseSkeleton.RIGHT_BACK_TIBIA);
				break;
			case 2:
				fServoOldPos[0] = context.getPosition(Spidy_BaseSkeleton.LEFT_BACK_COXA);
				fServoOldPos[1] = context.getPosition(Spidy_BaseSkeleton.LEFT_BACK_FEMUR);
				fServoOldPos[2] = context.getPosition(Spidy_BaseSkeleton.LEFT_BACK_TIBIA);
				fServoOldPos[3] = context.getPosition(Spidy_BaseSkeleton.RIGHT_FRONT_COXA);				
				fServoOldPos[4] = context.getPosition(Spidy_BaseSkeleton.RIGHT_FRONT_FEMUR);
				fServoOldPos[5] = context.getPosition(Spidy_BaseSkeleton.RIGHT_FRONT_TIBIA);
				break;
			case 4:
				fServoOldPos[0] = context.getPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_COXA);
				fServoOldPos[1] = context.getPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_FEMUR);
				fServoOldPos[2] = context.getPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_TIBIA);
				fServoOldPos[3] = context.getPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_COXA);				
				fServoOldPos[4] = context.getPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_FEMUR);
				fServoOldPos[5] = context.getPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_TIBIA);
				break;
			}
		}
		
		fStepTime+=fSlice;
		
		float fStep = (fStepTime>fStepPlacementDuration)?1:fStepTime/fStepPlacementDuration;
		
		switch(iStep)
		{
		case 0:
			context.setPosition(Spidy_BaseSkeleton.LEFT_FRONT_FEMUR,  lerp(fServoOldPos[1], -fFemurHigh, fStep));
			context.setPosition(Spidy_BaseSkeleton.LEFT_FRONT_TIBIA,  lerp(fServoOldPos[2], -fTibiaHigh, fStep));
			
			context.setPosition(Spidy_BaseSkeleton.RIGHT_BACK_FEMUR,  lerp(fServoOldPos[4], fFemurHigh, fStep));
			context.setPosition(Spidy_BaseSkeleton.RIGHT_BACK_TIBIA,  lerp(fServoOldPos[5], fTibiaHigh, fStep));
			break;
		case 1:
			context.setPosition(Spidy_BaseSkeleton.LEFT_FRONT_COXA,   lerp(fServoOldPos[0], -fCoxyHigh, fStep));
			context.setPosition(Spidy_BaseSkeleton.LEFT_FRONT_FEMUR,  lerp(-fFemurHigh, -fFemurLow, fStep));
			context.setPosition(Spidy_BaseSkeleton.LEFT_FRONT_TIBIA,  lerp(-fTibiaHigh, -fTibiaLow, fStep));
			
			context.setPosition(Spidy_BaseSkeleton.RIGHT_BACK_COXA,   lerp(fServoOldPos[3], fCoxyLow, fStep));
			context.setPosition(Spidy_BaseSkeleton.RIGHT_BACK_TIBIA,  lerp(fTibiaHigh, fTibiaLow, fStep));
			context.setPosition(Spidy_BaseSkeleton.RIGHT_BACK_FEMUR,  lerp(fFemurHigh, fFemurLow, fStep));
			break;
		case 2:
			context.setPosition(Spidy_BaseSkeleton.LEFT_BACK_FEMUR,  lerp(fServoOldPos[1], -fFemurHigh, fStep));
			context.setPosition(Spidy_BaseSkeleton.LEFT_BACK_TIBIA,  lerp(fServoOldPos[2], -fTibiaHigh, fStep));
			
			context.setPosition(Spidy_BaseSkeleton.RIGHT_FRONT_FEMUR,  lerp(fServoOldPos[4], fFemurHigh, fStep));
			context.setPosition(Spidy_BaseSkeleton.RIGHT_FRONT_TIBIA,  lerp(fServoOldPos[5], fTibiaHigh, fStep));
			break;
		case 3:
			context.setPosition(Spidy_BaseSkeleton.LEFT_BACK_COXA,   lerp(fServoOldPos[0], -fCoxyHigh, fStep));
			context.setPosition(Spidy_BaseSkeleton.LEFT_BACK_FEMUR,  lerp(-fFemurHigh, -fFemurLow, fStep));
			context.setPosition(Spidy_BaseSkeleton.LEFT_BACK_TIBIA,  lerp(-fTibiaHigh, -fTibiaLow, fStep));
			
			context.setPosition(Spidy_BaseSkeleton.RIGHT_FRONT_COXA,   lerp(fServoOldPos[3], +fCoxyLow, fStep));
			context.setPosition(Spidy_BaseSkeleton.RIGHT_FRONT_FEMUR,  lerp(fFemurHigh, fFemurLow, fStep));
			context.setPosition(Spidy_BaseSkeleton.RIGHT_FRONT_TIBIA,  lerp(fTibiaHigh, fTibiaLow, fStep));			
			break;
		case 4:
			context.setPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_FEMUR,  lerp(fServoOldPos[1], -fFemurHigh, fStep));
			context.setPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_TIBIA,  lerp(fServoOldPos[2], -fTibiaHigh, fStep));
			
			context.setPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_FEMUR,  lerp(fServoOldPos[4], fFemurHigh, fStep));
			context.setPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_TIBIA,  lerp(fServoOldPos[5], fTibiaHigh, fStep));
			break;
		case 5:
			context.setPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_COXA,   lerp(fServoOldPos[0], -fCoxyLow, fStep));
			context.setPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_FEMUR,  lerp(-fFemurHigh, -fFemurLow, fStep));
			context.setPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_TIBIA,  lerp(-fTibiaHigh, -fTibiaLow, fStep));

			context.setPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_COXA,   lerp(fServoOldPos[3], fCoxyHigh, fStep));
			context.setPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_FEMUR,  lerp(fFemurHigh, fFemurLow, fStep));
			context.setPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_TIBIA,  lerp(fTibiaHigh, fTibiaLow, fStep));			
			break;
		}
		// next step
		if(fStepTime>=fStepPlacementDuration)
		{
			fStepTime = 0.0f;
			iStep++;
		}
		// end animation ?
		if(iStep>5) return true;
		
		return false;
	}
}
